﻿using System;
//
// Position.cs
//
// Authors:
//    Claus Jørgensen <10229@iha.dk>
//    Jens Bloch Christensen <09584@iha.dk>
//    Jonas Daugaard Møller <08814@iha.dk>
//
using NIP.ControllerInterface;

namespace NIP.ControllerInterface
{
    /// <summary>
    /// Robot position structure, with mapped properties to the correct coordinate.
    /// </summary>
    public class Position
    {
        /// <summary>
        /// Axis rotations.
        /// </summary>
        private volatile int[] enc = new int[USBC.TotalAxes];

        /// <summary>
        /// Bone joints.
        /// </summary>
        private volatile int[] joint = new int[USBC.TotalAxes];
        private volatile double[] jointd = new double[USBC.TotalAxes];

        /// <summary>
        /// XYZ coordinates.
        /// Axis 1: X-coord in micrometers (ie. 50,000 = 5 cm)
        /// Axis 2: Y-coord in micrometers
        /// Axis 3: Z-coord in micrometers
        /// Axis 4: Pitch angle in degrees / 1,000 (ie. 90,000 = 90 degrees)
        /// Axis 5: Roll angle in degrees / 1,000
        /// It should be noted that (0, 0, 0, x, x) is at the bottom-center of the robot.
        /// Home is close to (169000, 0, 500000, -63000, 0). ***this seems wrong***
        /// </summary>
        private volatile int[] xyz = new int[USBC.TotalAxes];
        private volatile double[] xyzd = new double[USBC.TotalAxes];

        /// <summary>
        /// Gets or sets the axis rotations.
        /// </summary>
        public int[] Enc
        {
            get
            {
                return enc;
            }
            set
            {
                enc = value;
            }
        }

        /// <summary>
        /// Gets or sets the bone joints (micro degress: 1000 units = 1 degree).
        /// </summary>
        public int[] Joint
        {
            get
            {
                return joint;
            }
            set
            {
                //joint = value;
                for (int i = 0; i < USBC.TotalAxes; i++)
                {
                    joint[i] = value[i];
                    jointd[i] = (Convert.ToDouble(value[i]) / 1000.0);
                }
            }
        }

        /// <summary>
        /// Gets or sets the bone joints (degress: 1 degree = 1000 units ).
        /// </summary>
        public double[] Jointd
        {
            get
            {
                return jointd;
            }
            set
            {
                //jointd = value;
                for (int i = 0; i < USBC.TotalAxes; i++)
                {
                    jointd[i] = value[i];
                    joint[i] = Convert.ToInt32(value[i]*1000.0);
                }
            }
        }

        /// <summary>
        /// Gets or sets the coordinate translation in robot units (microns: 1000 units = 1mm).
        /// </summary>
        public int[] XYZ
        {
            get
            {
                return xyz;
            }
            set
            {
                //xyz = value;
                for (int i = 0; i < USBC.TotalAxes; i++)
                {
                    xyz[i] = value[i];
                    xyzd[i] = (Convert.ToDouble(value[i]) / 1000.0);
                }
            }
        }

        /// <summary>
        /// Gets or sets the coordinate translation (mm: 1mm = 1000 units).
        /// </summary>
        public double[] XYZd
        {
            get
            {
                return xyzd;
            }
            set
            {
                //xyzd = value;
                for (int i = 0; i < USBC.TotalAxes; i++)
                {
                    xyzd[i] = value[i];
                    xyz[i] = Convert.ToInt32(value[i]*1000.0);
                }
            }
        }

        /// <summary>
        /// Gets or sets the position of the torso (XYZ[1])
        /// </summary>
        public int Torso
        {
            get
            {
                return XYZ[0];
            }
            set
            {
                XYZ[0] = value;
            }
        }

        /// <summary>
        /// Gets or sets the position of the shoulder (XYZ[0])
        /// </summary>
        public int Shoulder
        {
            get
            {
                return XYZ[1];
            }
            set
            {
                XYZ[1] = value;
            }
        }

        /// <summary>
        /// Gets or sets the position of the elbow (XYZ[2])
        /// </summary>
        public int Elbow
        {
            get
            {
                return XYZ[2];
            }
            set
            {
                XYZ[2] = value;
            }
        }

        /// <summary>
        /// Gets or sets the position of the pitch (XYZ[3])
        /// </summary>
        public int WristPitch
        {
            get
            {
                return xyz[3];
            }
            //set
            //{
            //    XYZ[3] = value;
            //}
            set
            {
                //xyz = value;
                xyz[3] = value;
                xyzd[3] = (Convert.ToDouble(value) / 1000.0);               
            }
        }

        public double WristPitchd
        {
            get
            {
                return xyzd[3];
            }
            set
            {
                //xyzd = value;
                xyzd[3] = value;
                xyz[3] = Convert.ToInt32(value * 1000.0);
            }
        }

        /// <summary>
        /// Gets or sets the position of the roll (XYZ[4])
        /// </summary>
        public int WristRoll
        {
            get
            {
                return xyz[4];
            }
            //set
            //{
            //    XYZ[4] = value;
            //}
            set
            {
                //xyz = value;
                xyz[4] = value;
                xyzd[4] = (Convert.ToDouble(value) / 1000.0);               
            }
        }

        public double WristRolld
        {
            get
            {
                return xyzd[4];
            }
            set
            {
                //xyzd = value;
                xyzd[4] = value;
                xyz[4] = Convert.ToInt32(value * 1000.0);
            }
        }
        /// <summary>
        /// Gets or sets the offset (XYZ[5])
        /// </summary>
        public int Offset
        {
            get
            {
                return xyz[5];
            }
            set
            {
                xyz[5] = value;
                xyzd[5] = (Convert.ToDouble(value) / 1000.0);
            }
        }

        public double Offsetd
        {
            get
            {
                return xyzd[5];
            }
            set
            {
                xyzd[5] = value;
                xyz[5] = Convert.ToInt32(value * 1000.0);
            }
        }
    }
}
